/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

/*
 import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Victor;
 */

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.vision.Target;
import edu.wpi.first.wpilibj.vision.TrackerDashboard;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Victor;


public class RobotMain extends SimpleRobot {

    final int Preload = 1;
    final int Retract = 0;
    final int Latch = 1;
    final int Unlatch = 0;
    final int Left = 1;
    final int Right = 0;
    final int Forward = 1;
    final int Reverse = -1;
    final int Off = 0;
    final int Low = 0;
    final int High = 1;

    //ExtendedKickerSensor stuff
    final boolean Extended = false;
    final boolean Retracted = true;
    //boolean extendedKicker = Retracted;
    //boolean extendedKickerActive = true;
    //private DigitalInput ExtendedKickerSensor = new DigitalInput(8);
    //

    boolean ballPresent = false;
    boolean broken = false;
    private final int kDigitalSlot = 4;
    private final int kSolenoidSlot = 7;
    // Digital module = slot 7
    // Solenoid Module = slot 6
    // Analog Module = slot 5
    private Compressor compressor = new Compressor(kDigitalSlot, 14, kDigitalSlot, 8);
    private Joystick leftStick = new Joystick(1);
    private Joystick rightStick = new Joystick(2);
    private Joystick thirdStick = new Joystick(3);
    private Jaguar m_frontLeftMotor = new Jaguar(kDigitalSlot, 3);
    private Jaguar m_frontRightMotor = new Jaguar(kDigitalSlot, 1);
    private Jaguar m_rearLeftMotor = new Jaguar(kDigitalSlot, 4);
    private Jaguar m_rearRightMotor = new Jaguar(kDigitalSlot, 2);
    private Team997Drive drivetrain = new Team997Drive(m_frontLeftMotor, m_rearLeftMotor, m_frontRightMotor, m_rearRightMotor);//= new Team997Drive(2, 4, 1, 3);
    private TrackerDashboard trackerDashboard = new TrackerDashboard();
    private DigitalInput autobit[] = new DigitalInput[4];
    private DigitalInput breakBeam = new DigitalInput(kDigitalSlot, 5);//1 if broken
    private Victor ballHolder = new Victor(kDigitalSlot, 5);
    private Solenoid fireShooterSolenoid = new Solenoid(kSolenoidSlot, 1);
    //Solenoids hardware switched to single control wire.
    private Solenoid retractShooterSolenoid = new Solenoid(kSolenoidSlot, 2);
    private Solenoid latchSolenoid = new Solenoid(kSolenoidSlot, 3);
    //Solenoids hardware switched to single control wire.
    private Solenoid unlatchSolenoid = new Solenoid(kSolenoidSlot, 4);
    private Solenoid gearSolenoid = new Solenoid(kSolenoidSlot, 5);
    private boolean preloaded = false;
    private boolean latched = false;
    private Encoder leftEncoder = new Encoder(kDigitalSlot, 12, kDigitalSlot, 13, false, Encoder.EncodingType.k2X);
    private Encoder rightEncoder = new Encoder(kDigitalSlot, 10, kDigitalSlot, 11, false, Encoder.EncodingType.k2X);

    public RobotMain() {
        getWatchdog().setEnabled(false);
        compressor.start();
        autobit[0] = new DigitalInput(1);
        autobit[1] = new DigitalInput(2);
        autobit[2] = new DigitalInput(3);
        autobit[3] = new DigitalInput(4);
        breakBeam = new DigitalInput(6);
        //getWatchdog().feed();

        // We start being not preloaded and unlatched.
        fireShooterSolenoid.set(false);
        retractShooterSolenoid.set(true);
        unlatchSolenoid.set(true);
        latchSolenoid.set(false);
        gearSolenoid.set(false);
    }

    /**
     * Performs the autonomous.  Turns off the watchdog,
     * starts a seperate kicker thread for autonomous, gets the zone number,
     * and starts the specified zone's autonomous routine.
     */
    public void autonomous() {
        // Disable watchdog while in Autonomous.

        getWatchdog().setEnabled(false);
        gearSolenoid.set(false);
    }

    /**
     * Setups the button thread for performing the kick operations, then allows
     * control of the motors in this thread.  Feeds the watchdog.
     */
    public void operatorControl() {
        driveShooterSolenoid(Retract);
        getWatchdog().setEnabled(true);
        gearSolenoid.set(false);
        System.out.println(autobit[0].get() + " = output 1");
        System.out.println(autobit[1].get() + " = ouput 2");
        System.out.println(autobit[2].get() + " = output 3");
        System.out.println(autobit[3].get() + " = ouput 4");
        //System.out.println(currentZone() + " = Zone");
        buttonThread buttonThread = new buttonThread(this);
        buttonThread.start();
        while (true && isOperatorControl() && isEnabled()) {

                        //Shifts gear
            if(rightStick.getRawButton(4)){
                shiftGear(Low);
            }

            if(rightStick.getRawButton(5)){
                shiftGear(High);
            }
            drivetrain.tankDrive(getLeftStick(), getRightStick());
            getWatchdog().feed();
            Thread.yield();
        }
        // Releases presurrized kick.  Saf'ty code!
        makeReadyForSoftKick();
        try {
            buttonThread.join();
        } catch (InterruptedException ex) {
            System.out.println(ex.getMessage());
        }


    }

    //ExtendedKickerSensor stuff
    public void retractKicker()   {
            driveShooterSolenoid(Retract);
            Timer.delay(2);
            driveLatchSolenoid(Latch);
            Timer.delay(1);
            driveShooterSolenoid(Preload);
            Timer.delay(1);
    }

    /**
     * Gets the left joystick
     * @return The left joystick
     */
    public Joystick getLeftStick() {
        return leftStick;
    }

    /**
     * Gets the right joystick
     * @return The right joystick
     */
    public Joystick getRightStick() {
        return rightStick;
    }

    /**
     * Shifts gears.
     * @param gear The gear (low vs. high)
     */
    private void shiftGear(int gear) {
        if (gear == Low) {
            gearSolenoid.set(false);
        } else {
            gearSolenoid.set(true);
        }
    }

    /** 
     * Gets the robot ready for the big kick.
     */
    private void makeReadyForKick() {
        //Not latched and needs to be preloaded.
        if ((preloaded == false) && (latched == false)) {
            driveLatchSolenoid(Latch);
            Timer.delay(1);
            driveShooterSolenoid(Preload);
            Timer.delay(1);
        }
        //Latched, but needs to preload.
        if ((preloaded == false) && (latched == true)) {
            driveShooterSolenoid(Preload);
            Timer.delay(1);
        }
        //Preloaded, but needs to latch.
        if ((preloaded == true) && (latched == false)) {
            driveShooterSolenoid(Retract);
            Timer.delay(2);
            driveLatchSolenoid(Latch);
            Timer.delay(1);
            driveShooterSolenoid(Preload);
            Timer.delay(1);
        }
    }

    /**
     * Peforms the kick routine.  First loads the kicker if needed.
     * After kicking it loads the kicker for the next big kick.
     */
    public void performKick() {
        makeReadyForKick();
        //Kick!
        driveLatchSolenoid(Unlatch);
        Timer.delay(.5); // If this changed, change the delay in autoKick!!!
        //Reload for big kick.
        driveShooterSolenoid(Retract);
        Timer.delay(2);
        driveLatchSolenoid(Latch);
        Timer.delay(1);
        driveShooterSolenoid(Preload);
        Timer.delay(1);
        System.out.println("Kick Performed");
    }

    /**
     * Gets the robot ready to perform a soft kick.
     */
    private void makeReadyForSoftKick() {
        //Loaded and latched for big kick, so let's release it.
        if ((preloaded == true) && (latched == true)) {
            driveShooterSolenoid(Retract);
            Timer.delay(1); // If this changed, change the delay in autoSoftKick!!!
            driveLatchSolenoid(Unlatch);
            Timer.delay(.5); // If this changed, change the delay in autoSoftKick!!!
        }
        //Latched, but not loaded.
        if ((preloaded == false) && (latched == true)) {
            driveLatchSolenoid(Unlatch);
            Timer.delay(.5);
        }
        //Preloaded and needs to be brought back.
        if ((preloaded == true) && (latched == false)) {
            driveShooterSolenoid(Retract);
            Timer.delay(2);
        }
    }

    /**
     * Performs a soft kick by releasing the depessurizing the kicker,
     * unlatching, and the repressurizing the unlatched kicker.
     */
    public void performSoftKick() {
        makeReadyForSoftKick();
        //Kick!
        driveShooterSolenoid(Preload);
        Timer.delay(.5); // If this changed, change the delay in autoSoftKick!!!
        //Reload for soft kick
        driveShooterSolenoid(Retract);
        Timer.delay(2);
        // Do we really want to do this? Or should we comment this out and assume
        // we are in zone 1 so the next kick will be a soft one?
//        driveLatchSolenoid(Latch);
//        Timer.delay(1);
//        driveShooterSolenoid(Preload);
//        Timer.delay(1);
    }


    /**
     * Calculates the number of encoder pulses required to travel a specified
     * distance.
     * @param inches Number of inches.
     * @return Number of encoder pulses to travel the specified distance.
     */
    private int convertInchesToEncoderPulses(double inches) {
        return (int) ((inches - 1.5) / 0.04);
    }

    /**
     * Drives straight a specified distance
     * @param distanceInches Distance in inches.
     * @param direction Forward or backward.
     */
    public void goToBall(double maxDistanceInches) {
        int maxDistancePulse = 0;
        if (maxDistanceInches > 6) {
            maxDistancePulse = convertInchesToEncoderPulses(maxDistanceInches);
        }
        double angle = 0.0;
        leftEncoder.reset();
        boolean breakFlag = false;
        while (!breakBeam.get() && !breakFlag && (leftEncoder.get() < maxDistancePulse)) {
            drivetrain.drive(-.5, -angle);
            if (breakBeam.get()) {
                breakFlag = true;
            }
            Thread.yield();
        }
        drivetrain.drive(0, 0);
    }

    /**
     * Rotates the robot in the specified direction
     * @param angle Angle of rotation
     * @param direction left or right
     */
    public void turn(double angle, int direction) { // successive turns fail
        System.out.println("Turn" + angle + direction);
        if (direction == Left) {
            drivetrain.setLeftRightMotorSpeeds(.7, -.7);
            Thread.yield();
        } else {
            drivetrain.setLeftRightMotorSpeeds(-.7, .7);
            Thread.yield();
        }
        drivetrain.setLeftRightMotorSpeeds(0, 0);
    }

    /**
     * Drives the shooter solenoid
     * @param direction 1 preloaded 0 flush
     */
    public void driveShooterSolenoid(int direction) {
        if (direction == 1) {
            retractShooterSolenoid.set(false);
            fireShooterSolenoid.set(true);
            preloaded = true;
        } else {
            fireShooterSolenoid.set(false);
            retractShooterSolenoid.set(true);
            preloaded = false;
        }
    }

    /**
     * Drives the latch solenoid.
     * @param direction 1 latched 0 retract
     */
    public void driveLatchSolenoid(int direction) {
        if (direction == 1) {
            unlatchSolenoid.set(false);
            latchSolenoid.set(true);
            latched = true;
        } else {
            latchSolenoid.set(false);
            unlatchSolenoid.set(true);
            latched = false;
        }
    }

    /**
     * Toggles the ball holder on and off.
     */
    public void switchBallHolder(int direction) {
        if (direction == Forward) {
            ballHolder.set(-1.0);
            System.out.println("Ball Holder Forward");
        } else if (direction == Off) {
            System.out.println("Ball Holder Off");
            ballHolder.set(0);
        } else {
            ballHolder.set(1.0);
            System.out.println("Ball Holder Reverse");
        }
    }

    /**
     * Kicks a ball when the ball sensor sees it.  Only exits the method
     * when the button is released.
     * @param stick The joysick that has a button pressed
     * @param buttonNumber The number of button being pressed.
     */
    public void sensorKick(Joystick stick, int buttonNumber) {
        makeReadyForKick();
        while (stick.getRawButton(buttonNumber)) {
            if (breakBeam.get()) {
                performKick();
            }
            Thread.yield();
        }
    }


    /**
     * Gets the left encoder object.
     * @return The left encoder
     */
    public Encoder getLeftEncoder() {
        return leftEncoder;
    }

    /**
     * Gets the right encoder object.
     * @return The right encoder
     */
    public Encoder getRightEncoder() {
        return rightEncoder;
    }

    /**
     * @return the thirdStick
     */
    public Joystick getThirdStick() {
        return thirdStick;
    }
}
